#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

#define MAX_POINT_NUM 500
#define MIN_INTERV_MS 5

typedef struct _move_param
{
    int relative_position;//steps
    int start_speed;//steps/s
    int max_speed;//steps/s
    int inc_speed;//steps/s2
    int dec_speed;//steps/s2
}move_param;

typedef struct _pos_speed
{
    int speed;
    int position;
}pos_speed;

int generate_ladder_curve(move_param* moveParamIn, int * numPointsInc, pos_speed * posSpeedInc,int * numPointsDec, pos_speed * posSpeedDec)
{
    int half_position = ((moveParamIn->relative_position)>>1); 
    int timeInc_s = (moveParamIn->max_speed - moveParamIn->start_speed)/(moveParamIn->inc_speed);
    int timeInc_ms = timeInc_s*1000;
    int incPointsNum =  timeInc_ms/MIN_INTERV_MS;
    int interv_ms = 0;
    if(incPointsNum > MAX_POINT_NUM)
    {
        incPointsNum = MAX_POINT_NUM;
        interv_ms = timeInc_ms/MAX_POINT_NUM;

    } 
    else
    {
        interv_ms = MIN_INTERV_MS;
    }
    
    //v0*t + a*t*t/2
    int posStopInc = moveParamIn->start_speed * timeInc_s + ((moveParamIn->inc_speed*timeInc_s*timeInc_s)>>1);
    int speedStopInc = moveParamIn->max_speed;
    if(half_position < posStopInc)
    {
        posStopInc = half_position;
    }


    int timeDec_s = moveParamIn->max_speed/(moveParamIn->dec_speed);
    int timeDec_ms = timeDec_s*1000;
    int decPointsNum =  timeDec_ms/MIN_INTERV_MS;
    int interv_ms_dec = 0;
    if(decPointsNum > MAX_POINT_NUM)
    {
        decPointsNum = MAX_POINT_NUM;
        interv_ms_dec = timeDec_ms/MAX_POINT_NUM;

    } 
    else
    {
        interv_ms_dec = MIN_INTERV_MS;
    }
    
    int posStartDec = moveParamIn->relative_position - 
    ((moveParamIn->dec_speed*timeDec_ms/1000*timeDec_ms)>>1)/1000;

    if(posStartDec<half_position)
    {
        posStartDec = half_position;
    }
    
    posSpeedInc[0].position = 0;
    timeInc_ms = 0;
    posSpeedInc[0].speed = moveParamIn->start_speed;
    //int  * speedIncBUff =  new int[incPointsNum];
    int i = 0;

    for(i=1;i<incPointsNum;i++)
    {
       timeInc_ms += interv_ms; 
       posSpeedInc[i].position = moveParamIn->start_speed * timeInc_ms / 1000  +  ((moveParamIn->inc_speed * timeInc_ms/1000 * timeInc_ms)>>1)/1000;
       if(i==147)
       {
           printf("timeInc_ms:%d, v0t=%d, at2/2=%d\n",
           timeInc_ms,
           moveParamIn->start_speed * timeInc_ms / 1000,
           ((moveParamIn->inc_speed * timeInc_ms/1000 * timeInc_ms)>>1)/1000);
       }
       posSpeedInc[i].speed = moveParamIn->start_speed  + moveParamIn->inc_speed * timeInc_ms /1000;
       if(posSpeedInc[i].position >= half_position) 
       {
           speedStopInc = posSpeedInc[i].speed;
           break;
       }
    }
    if(i==incPointsNum)
    {
       speedStopInc =  posSpeedInc[i-1].speed;
    }
    else
    {
       incPointsNum = i;
       speedStopInc =  posSpeedInc[i].speed;
       
    }

    * numPointsInc = incPointsNum; 
    
  
    printf("speedStopInc:%d, posStopInc:%d,posStartDec:%d\n",speedStopInc,posStopInc,posStartDec);
    
    posSpeedDec[0].position = posStartDec;
    timeDec_ms = 0;
    posSpeedDec[0].speed = speedStopInc;
    //int * speedDecBUff = new int[decPointsNum];
    for(i=1;i<decPointsNum;i++)
    {
       timeDec_ms += interv_ms_dec; 
       posSpeedDec[i].position = posStartDec
                       + speedStopInc * timeDec_ms / 1000 
                       - ((moveParamIn->dec_speed * timeDec_ms/1000 * timeDec_ms)>>1)/1000;
       posSpeedDec[i].speed = speedStopInc - moveParamIn->dec_speed * timeDec_ms /1000;
       
       if(posSpeedDec[i].position >= moveParamIn->relative_position) 
       {
           break;
       }
    }

    if(i==decPointsNum)
    {
       //speedStopInc =  speedIncBUff[i-1];
       * numPointsDec = decPointsNum;
    }
    else
    {
       * numPointsDec = i;
    }

    printf("interv_inc:%d, interv_dec:%d\n",interv_ms,interv_ms_dec);
}


int generate_ladder_curve_f(move_param* moveParamIn, int * numPointsInc, pos_speed * posSpeedInc,int * numPointsDec, pos_speed * posSpeedDec)
{
    float half_position = (float)(moveParamIn->relative_position)/2;//((moveParamIn->relative_position)>>1) + 
    //(moveParamIn->relative_position&0x0001)?0.5:0.0; 
    float timeInc_s = ((float)(moveParamIn->max_speed - moveParamIn->start_speed))/((float)(moveParamIn->inc_speed));
    float timeInc_ms = timeInc_s*1000;
    int incPointsNum =  (int)(timeInc_ms/MIN_INTERV_MS);
    int interv_ms = 0;
    if(incPointsNum > MAX_POINT_NUM)
    {
        incPointsNum = MAX_POINT_NUM;
        interv_ms = (int)(timeInc_ms/MAX_POINT_NUM);

    } 
    else
    {
        interv_ms = MIN_INTERV_MS;
    }

    //printf("111,incPointsNum = %d,interv_ms = %d\n",incPointsNum,interv_ms);
    
    //v0*t + a*t*t/2
    float posStopInc = moveParamIn->start_speed * timeInc_s + ((moveParamIn->inc_speed*timeInc_s*timeInc_s)/2);
    int speedStopInc = moveParamIn->max_speed;
    //printf("half_position:%f,posStopInc:%f\n",half_position,posStopInc);
    if(half_position < posStopInc)
    {
        posStopInc = half_position;
    }
    
    posSpeedInc[0].position = 0;
    timeInc_ms = 0;
    posSpeedInc[0].speed = moveParamIn->start_speed;
    //int  * speedIncBUff =  new int[incPointsNum];
    int i = 0;

    for(i=1;i<incPointsNum;i++)
    {
       timeInc_ms += interv_ms; 
       timeInc_s = timeInc_ms/1000;
       posSpeedInc[i].position = (int)(moveParamIn->start_speed * timeInc_s +  moveParamIn->inc_speed * timeInc_s * timeInc_s/2);
    //    if(i==147)
    //    {
    //        printf("timeInc_ms:%f, v0t=%f, at2/2=%f\n",
    //        timeInc_ms,
    //        moveParamIn->start_speed * timeInc_s,
    //        (moveParamIn->inc_speed * timeInc_s * timeInc_s));
    //    }
       posSpeedInc[i].speed = (int)(moveParamIn->start_speed  + moveParamIn->inc_speed * timeInc_s);
       if(posSpeedInc[i].position >= half_position) 
       {
           speedStopInc = posSpeedInc[i].speed;
           break;
       }
    }
    if(i==incPointsNum)
    {
       speedStopInc =  posSpeedInc[i-1].speed;
    }
    else
    {
       incPointsNum = i;
       speedStopInc =  posSpeedInc[i].speed;
       
    }

    * numPointsInc = incPointsNum;
    
    float timeDec_s = ((float)(speedStopInc))/((float)(moveParamIn->dec_speed));
    float timeDec_ms = timeDec_s*1000;
    int decPointsNum =  (int)(timeDec_ms/MIN_INTERV_MS);
    int interv_ms_dec = 0;
    if(decPointsNum > MAX_POINT_NUM)
    {
        decPointsNum = MAX_POINT_NUM;
        interv_ms_dec = (int)(timeDec_ms/MAX_POINT_NUM);
    } 
    else
    {
        interv_ms_dec = MIN_INTERV_MS;
    }
    
    float posStartDec = moveParamIn->relative_position - 
    (moveParamIn->dec_speed*timeDec_s*timeDec_s)/2;

    if(posStartDec<half_position)
    {
        posStartDec = half_position;
    } 
    
  
    printf("speedStopInc:%d, posStopInc:%f,posStartDec:%f\n",speedStopInc,posStopInc,posStartDec);
    
    posSpeedDec[0].position = posStartDec;
    timeDec_ms = 0;
    posSpeedDec[0].speed = speedStopInc;
    //int * speedDecBUff = new int[decPointsNum];
    for(i=1;i<decPointsNum;i++)
    {
       timeDec_ms += interv_ms_dec;
       timeDec_s = timeDec_ms/1000;
       posSpeedDec[i].position = (int)(posStartDec
                       + speedStopInc * timeDec_s
                       - moveParamIn->dec_speed * timeDec_s * timeDec_s/2);
       posSpeedDec[i].speed = speedStopInc - moveParamIn->dec_speed * timeDec_s;
       
       if(posSpeedDec[i].position >= moveParamIn->relative_position) 
       {
           break;
       }
    }

    if(i==decPointsNum)
    {
       //speedStopInc =  speedIncBUff[i-1];
       * numPointsDec = decPointsNum;
    }
    else
    {
       * numPointsDec = i;
    }

    printf("interv_inc:%d, interv_dec:%d\n",interv_ms,interv_ms_dec);
}


int generate_ladder_curve_f1(move_param* moveParamIn, int * numPointsInc, pos_speed * posSpeedInc,int * numPointsDec, pos_speed * posSpeedDec)
{
    
    // float posMin = (float)((moveParamIn->max_speed*moveParamIn->max_speed
    //              - moveParamIn->start_speed*moveParamIn->start_speed)
    //              *(moveParamIn->inc_speed + moveParamIn->dec_speed))
    //              /(float)(moveParamIn->inc_speed*moveParamIn->dec_speed*2);
    
    float a1a2Param = (float)(moveParamIn->inc_speed + moveParamIn->dec_speed)
                 /(float)(moveParamIn->inc_speed*moveParamIn->dec_speed*2);

    float v02 = (float)(moveParamIn->start_speed*moveParamIn->start_speed);
    float vxv0 = (float)(moveParamIn->max_speed*moveParamIn->max_speed - v02);

    float posMin = a1a2Param * vxv0;

    float Vmax = moveParamIn->max_speed;

    //printf("posMin:%f, a1a2Param:%f, vxv0:%f\n",posMin,a1a2Param,vxv0);
    if(posMin > moveParamIn->relative_position)
    {
        a1a2Param = 1.0/a1a2Param;
        Vmax = sqrtf(a1a2Param*moveParamIn->relative_position + v02);
    }
    //printf("Vmax:%f,a1a2Param:%f,v02:%f\n",Vmax,a1a2Param,v02);

    
    
    //float half_position = (float)(moveParamIn->relative_position)/2;//((moveParamIn->relative_position)>>1) + 
    //(moveParamIn->relative_position&0x0001)?0.5:0.0; 
    float timeInc_s = ((float)(Vmax - moveParamIn->start_speed))/((float)(moveParamIn->inc_speed));
    float timeInc_ms = timeInc_s*1000;
    int incPointsNum =  (int)(timeInc_ms/MIN_INTERV_MS);
    int interv_ms = 0;
    if(incPointsNum > MAX_POINT_NUM)
    {
        incPointsNum = MAX_POINT_NUM;
        interv_ms = (int)(timeInc_ms/MAX_POINT_NUM);

    } 
    else
    {
        interv_ms = MIN_INTERV_MS;
    }

    //printf("111,incPointsNum = %d,interv_ms = %d\n",incPointsNum,interv_ms);
    
    //v0*t + a*t*t/2
    float posStopInc = moveParamIn->start_speed * timeInc_s + ((moveParamIn->inc_speed*timeInc_s*timeInc_s)/2);
    int speedStopInc = Vmax;
    //printf("half_position:%f,posStopInc:%f\n",half_position,posStopInc);
    
    posSpeedInc[0].position = 0;
    timeInc_ms = 0;
    posSpeedInc[0].speed = moveParamIn->start_speed;
    //int  * speedIncBUff =  new int[incPointsNum];
    int i = 0;

    for(i=0;i<incPointsNum;i++)
    {
       timeInc_ms += interv_ms; 
       timeInc_s = timeInc_ms/1000;
       posSpeedInc[i].position = (int)(moveParamIn->start_speed * timeInc_s +  moveParamIn->inc_speed * timeInc_s * timeInc_s/2);
    //    if(i==147)
    //    {
    //        printf("timeInc_ms:%f, v0t=%f, at2/2=%f\n",
    //        timeInc_ms,
    //        moveParamIn->start_speed * timeInc_s,
    //        (moveParamIn->inc_speed * timeInc_s * timeInc_s));
    //    }
       posSpeedInc[i].speed = (int)(moveParamIn->start_speed  + moveParamIn->inc_speed * timeInc_s);
       if(posSpeedInc[i].position >= posStopInc) 
       {
           printf("i=%d,position[%d],posStopInc[%f]\n",
           i,posSpeedInc[i].position,posStopInc);
           speedStopInc = posSpeedInc[i].speed;
           break;
       }
    }
     if(i==incPointsNum)
     {
        speedStopInc =  posSpeedInc[i-1].speed;
     }
     else
     {
         if(i<(incPointsNum-1))
         {
             incPointsNum = i;
             speedStopInc =  posSpeedInc[i].speed;
         }       
     }

    * numPointsInc = incPointsNum;
    
    float timeDec_s = ((float)(Vmax-moveParamIn->start_speed))/((float)(moveParamIn->dec_speed));
    float timeDec_ms = timeDec_s*1000;
    int decPointsNum =  (int)(timeDec_ms/MIN_INTERV_MS);
    int interv_ms_dec = 0;
    printf("timeDec_s:%f,decPointsNum:%d\n",timeDec_s,decPointsNum);
    if(decPointsNum > MAX_POINT_NUM)
    {
        decPointsNum = MAX_POINT_NUM;
        interv_ms_dec = (int)(timeDec_ms/MAX_POINT_NUM);
    } 
    else
    {
        interv_ms_dec = MIN_INTERV_MS;
    }
    
    float posStartDec = moveParamIn->relative_position +  
    (moveParamIn->dec_speed*timeDec_s*timeDec_s)/2 - Vmax*timeDec_s;
 
    
  
    printf("speedStopInc:%d, posStopInc:%f,posStartDec:%f\n",speedStopInc,posStopInc,posStartDec);
    
    posSpeedDec[0].position = posStartDec;
    timeDec_ms = 0;
    posSpeedDec[0].speed = speedStopInc;
    //int * speedDecBUff = new int[decPointsNum];
    for(i=0;i<decPointsNum;i++)
    {
       timeDec_ms += interv_ms_dec;
       timeDec_s = timeDec_ms/1000;
       posSpeedDec[i].position = (int)(posStartDec
                       + speedStopInc * timeDec_s
                       - moveParamIn->dec_speed * timeDec_s * timeDec_s/2);
       posSpeedDec[i].speed = speedStopInc - moveParamIn->dec_speed * timeDec_s;
       
       if(posSpeedDec[i].position >= moveParamIn->relative_position) 
       {
           break;
       }
    }

    * numPointsDec = decPointsNum;

    
    if(i<(decPointsNum-1))
    {
        * numPointsDec = i;
    }
       
    

    printf("interv_inc:%d, interv_dec:%d\n",interv_ms,interv_ms_dec);
}

int main(int argc,char* argv[])
{
    pos_speed * posSpeedInc = new pos_speed[MAX_POINT_NUM];

    pos_speed * posSpeedDec = new pos_speed[MAX_POINT_NUM];
    move_param moveParam;

    memset(&moveParam,0,sizeof(move_param));


    memset(posSpeedInc,0,sizeof(pos_speed)*MAX_POINT_NUM);
    memset(posSpeedDec,0,sizeof(pos_speed)*MAX_POINT_NUM);

    moveParam.dec_speed = 4000;
    moveParam.inc_speed = 4000;
    moveParam.relative_position = 8000;
    moveParam.max_speed = 4000;
    moveParam.start_speed = 1000;


    int numPointsInc = 0;
    int numPointsDec = 0;

    generate_ladder_curve_f1(&moveParam,&numPointsInc,posSpeedInc,&numPointsDec,posSpeedDec);

    printf("inc num:%d, dec num:%d\n",numPointsInc,numPointsDec);

    FILE * fp = fopen("curve_result.txt","wr");

    char buff[64];
    int ret = 0;
    int i=0;    
    if(fp == NULL)
    {

    }
    else
    {
        memset(buff,0,64);
        ret = sprintf(buff,"num inc points acc%d\n",numPointsInc);
        fwrite(buff,ret,1,fp);

        for(i=0;i< numPointsInc;i++)
        {
            memset(buff,0,64);
            ret = sprintf(buff,"%d\t%d\t%d\n",i,posSpeedInc[i].speed,posSpeedInc[i].position);
            fwrite(buff,ret,1,fp);
        }

        memset(buff,0,64);
        ret = sprintf(buff,"num inc points dec:%d\n",numPointsDec);
        fwrite(buff,ret,1,fp);


        for(i=0;i< numPointsDec;i++)
        {
            memset(buff,0,64);
            ret = sprintf(buff,"%d\t%d\t%d\n",i,posSpeedDec[i].speed,posSpeedDec[i].position);
            fwrite(buff,ret,1,fp);
        }

        fclose(fp);
    }
    
    


    delete [] posSpeedInc;
    delete [] posSpeedDec;


}